// Created by az on 2020/6/20.
//

#ifndef WMR_BEZIER_PLANNER_MAP_GENERATOR_H
#define WMR_BEZIER_PLANNER_MAP_GENERATOR_H

#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <mutex>

#include "utils.h"
#include "sdf_tools/collision_map.hpp"

#define MAP_PATH "/home/jg/catkin_ws/btraj/wmr_bezier_planner/map/gazebo_willowgarage_index.map";

enum PLANSTYLE {GLOBAL = 0, LOCAL};

class MAP_GENERATOR {
protected:
    double resolution_, inv_resolution_, map_margin_;
    double inflate_size_, MAX_VEL_;
    double x_local_size_, y_local_size_;
    double x_min_, x_max_, y_min_, y_max_;
    bool   has_map_, is_mapping_;
    std::string file_name_;
    std::ifstream fp_;
    std::ofstream wf_;

    std::mutex    file_lock_, imap_lock_;
    std::vector<std::pair<Eigen::Vector3d, bool >> vec_pt_;

    sdf_tools::COLLISION_CELL free_cell_;
    sdf_tools::COLLISION_CELL obst_cell_;
    sdf_tools::CollisionMapGrid * collision_map_;
    sdf_tools::CollisionMapGrid * inflate_map_;
    sdf_tools::CollisionMapGrid * collision_map_local_;

    Eigen::Vector3d map_origin_, local_origin_;

    const float lofree = -1, looccu = 100;
    const float oddsmax = 128, oddsmin = -127;
public:
    MAP_GENERATOR( double x_lsize, double y_lsize, double resolution,
            bool is_map = false ,double margin = 0, double maxv = 1, double inflate = 0.3)
    :  has_map_(false) {
        file_name_    = MAP_PATH;
        x_local_size_ = x_lsize;
        y_local_size_ = y_lsize;
        map_margin_   = margin;
        MAX_VEL_      = maxv;
        inflate_size_ = inflate;
        is_mapping_   = is_map;

        x_min_ = x_max_ = y_min_ = y_max_ = 0;

        resolution_     = resolution;
        inv_resolution_ = 1.0 / resolution_;

        collision_map_       = new sdf_tools::CollisionMapGrid();
        inflate_map_         = new sdf_tools::CollisionMapGrid();
        collision_map_local_ = new sdf_tools::CollisionMapGrid();

        loadFile();
    }

    MAP_GENERATOR(std::string _file,  double x_lsize, double y_lsize, double resolution,
            bool is_map = false ,double margin = 0, double maxv = 1, double inflate = 0.3)
    : has_map_(false) {
        file_name_    = std::move(_file);
        x_local_size_ = x_lsize;
        y_local_size_ = y_lsize;
        map_margin_   = margin;
        MAX_VEL_      = maxv;
        inflate_size_ = inflate;
        is_mapping_   = is_map;

        x_min_ = x_max_ = y_min_ = y_max_ = 0;

        resolution_     = resolution;
        inv_resolution_ = 1.0 / resolution_;

        collision_map_       = new sdf_tools::CollisionMapGrid();
        inflate_map_         = new sdf_tools::CollisionMapGrid();
        collision_map_local_ = new sdf_tools::CollisionMapGrid();

        loadFile();
    }

    ~MAP_GENERATOR(){
        //write collision_map_'s data to file
        if(is_mapping_) writeFile();

        fp_.close();
        wf_.close();

        vec_pt_.clear();
    }

    void builtLocalMap(Eigen::Vector3d start_pt);
    void refreshMap(double _x, double _y, std::vector<std::pair<Eigen::Vector3d, bool >> _vec_point);
    bool checkCoordObs(Eigen::Vector3d checkPt);

    sdf_tools::CollisionMapGrid * getGlobalMap();
    sdf_tools::CollisionMapGrid * getLocalMap();
    Eigen::Vector3d getMapOrigin();
    Eigen::Vector3d getLocalOrigin();
    std::vector<int64_t> location2Index(Eigen::Vector3d location, PLANSTYLE style);
    std::vector<double>  getMapSize();

protected:
    void pointInflate();
    void loadFile();
    void writeFile();
    void builtGlobalMap();
    void reMappingGMap(Eigen::Vector3d pt);
    void bresenHamLine(int x0, int y0, int x1, int y1);
};
#endif //WMR_BEZIER_PLANNER_MAP_GENERATOR_H
